#pragma config(Sensor, in1,    Potentiometer,  sensorPotentiometer)
#pragma config(Sensor, dgtl1,  rightEncoder,   sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  leftEncoder,    sensorQuadEncoder)
#pragma config(Sensor, dgtl8,  Sonar,          sensorSONAR_inch)
#pragma config(Motor,  port2,           rightMotor,    tmotorVex393, openLoop, reversed)
#pragma config(Motor,  port3,           leftMotor,     tmotorVex393, openLoop)
#pragma config(Motor,  port7,           clawMotor,     tmotorServoStandard, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

task main()
{
	wait1Msec(1000);							// Robot waits for 2000 milliseconds before executing program

	SensorValue[rightEncoder] = 0;	  // Set the encoder so that it starts counting at 0
	SensorValue[leftEncoder]  = 0;	  // Set the encoder so that it starts counting at 0

	while(SensorValue[rightEncoder]<4248 || SensorValue[leftEncoder]<4248)		// Creates an infinite loop, since "true" always evaluates to true
	{
		if(SensorValue[rightEncoder] == SensorValue[leftEncoder]) // If rightEncoder has counted the same amount as leftEncoder:
		{
			// Move Forward
			motor[rightMotor] = 100;		    // Right Motor is run at power level 100
			motor[leftMotor]  = 100;		    // Left Motor is run at power level 100
		}
		else if(SensorValue[rightEncoder] > SensorValue[leftEncoder])	// If rightEncoder has counted more encoder counts
		{
			// Turn slightly right
			motor[rightMotor] = 50;		    // Right Motor is run at power level 50
			motor[leftMotor]  = 100;		    // Left Motor is run at power level 100
		}
		else	// Only runs if leftEncoder has counted more encoder counts
		{
			// Turn slightly left
			motor[rightMotor] = 100;		    // Right Motor is run at power level 100
			motor[leftMotor]  = 50;		    // Left Motor is run at power level 50
		}
	}

	motor[leftMotor]=0;
	motor[rightMotor]=0;
	wait1Msec(1000);							            //Robot waits for 2000 milliseconds before executing program

	SensorValue[rightEncoder] = 0;	          //Set the encoder so that it starts counting at 0
	SensorValue[leftEncoder] = 0;	            //Set the encoder so that it starts counting at 0

	while(SensorValue[rightEncoder]> -365 || SensorValue[leftEncoder]< 365)
	{
		motor[leftMotor] = 100;
		motor[rightMotor] = -100;

		if(SensorValue[rightEncoder]< -365) {
			motor[rightMotor] = 0;
		}
		if(SensorValue[leftEncoder]> 365) {
			motor[leftMotor] = 0;
		}
	}
	motor[leftMotor]=0;
	motor[rightMotor]=0;
	wait1Msec(1000);

	SensorValue[rightEncoder] = 0;	  // Set the encoder so that it starts counting at 0
	SensorValue[leftEncoder]  = 0;	  // Set the encoder so that it starts counting at 0


	motor[clawMotor]=-127;
	while(SensorValue[Sonar]>=6 || SensorValue[Sonar]<=0)
	{
		if(SensorValue[rightEncoder] == SensorValue[leftEncoder]) // If rightEncoder has counted the same amount as leftEncoder:
		{
			// Move Forward
			motor[rightMotor] = 100;		    // Right Motor is run at power level 100
			motor[leftMotor]  = 100;		    // Left Motor is run at power level 100
		}
		else if(SensorValue[rightEncoder] > SensorValue[leftEncoder])	// If rightEncoder has counted more encoder counts
		{
			// Turn slightly right
			motor[rightMotor] = 50;		    // Right Motor is run at power level 50
			motor[leftMotor]  = 100;		    // Left Motor is run at power level 100
		}
		else	// Only runs if leftEncoder has counted more encoder counts
		{
			// Turn slightly left
			motor[rightMotor] = 100;		    // Right Motor is run at power level 100
			motor[leftMotor]  = 50;		    // Left Motor is run at power level 50
		}
	}

	motor[rightMotor]=0;
	motor[leftMotor]=0;
	wait1Msec(1000);

	motor[clawMotor]=127;

}
